#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <iostream>

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "tf2_msgs/msg/tf_message.hpp"
#include "std_msgs/msg/int8.hpp"
#include "std_msgs/msg/int16.hpp"
#include "math.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "tf2/LinearMath/Transform.h"

using namespace std::chrono_literals;
using std::placeholders::_1;

/* This example creates a subclass of Node and uses std::bind() to register a
* member function as a callback from the timer. */

typedef struct __point
{
  double x;
  double y;
  double yaw;
}point_TypeDef;

point_TypeDef position = {};
point_TypeDef planning_points[4] = {{-1.0f, 1.0f, 0.0f},
                                    {-5.0f, 2.0f, 0.0f},
                                    {-0.5f, 6.0f, 0.0f}, 
                                    {0.0f, 6.0f, 0.0f}};

int8_t game_state = 4;
int16_t robot_hp = 400;

class PlanningModule : public rclcpp::Node
{
  public:
    PlanningModule()
    : Node("planning_module"), count_(0)
    {
      subscription_tf = this->create_subscription<tf2_msgs::msg::TFMessage>(
      "tf", 10, std::bind(&PlanningModule::topic_callback, this, _1));
      subscription_hp = this->create_subscription<std_msgs::msg::Int16>(
      "robot_hp", 10, std::bind(&PlanningModule::topic_hp_callback, this, _1));
      publisher_cmd = this->create_publisher<geometry_msgs::msg::Twist>("move_cmd", 10);
      timer_ = this->create_wall_timer(
      500ms, std::bind(&PlanningModule::timer_callback, this));
    }

  private:

    void timer_callback()
    {
      static int8_t step = 0;
      auto message = geometry_msgs::msg::Twist();
      double vx = planning_points[step].x - position.x;
      double vy = planning_points[step].y - position.y;
      double v = sqrt(vx*vx + vy*vy);
      vx = vx / v ;
      vy = vy / v ;
      //double vz = 1;
      if(game_state == 0)
      {
        message.linear.x = 0;
        message.linear.y = 0;
        message.angular.z = 0;
      }
      else if(game_state == 4)
      {
        if (robot_hp > 150)
        {
          if(step != 1)
          {
            if(fabs(planning_points[step].x - position.x) + fabs(planning_points[step].y - position.y) < 0.5)
            {
              step = (step + 1) % 4;
            }
          }
        }
        else
        {
          if(fabs(planning_points[step].x - position.x) + fabs(planning_points[step].y - position.y) < 0.5)
          {
            if(step != 0)
            {
              step = (step + 1) % 4;
            }
          }
        }
        message.linear.x = + cos(position.yaw) * vx + sin(position.yaw) * vy;
        message.linear.y = - sin(position.yaw) * vx + cos(position.yaw) * vy;
      }
      publisher_cmd->publish(message);
    }
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_cmd;

    void topic_callback(const tf2_msgs::msg::TFMessage & msg) const
    {
      for (const auto& transform : msg.transforms) {
        tf2::Quaternion tf_rotation(transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w);
        tf2::Matrix3x3 rotationMatrix;
        rotationMatrix.setRotation(tf_rotation);
        position.x = transform.transform.translation.y;
        position.y = transform.transform.translation.x;
        position.yaw = atan2(rotationMatrix[0][1], rotationMatrix[0][0]);
        
        RCLCPP_INFO(this->get_logger(), "position: x=%f, y=%f, yaw=%f",
                    position.x,
                    position.y,
                    position.yaw);
      }
    }

    void topic_hp_callback(const std_msgs::msg::Int16 & msg) const
    {
      robot_hp = msg.data;
      std::cout << robot_hp << std::endl;
    }
    
    void topic_state_callback(const std_msgs::msg::Int16 & msg) const
    {
      game_state = msg.data;
      std::cout << game_state << std::endl;
    }

    rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription_tf;
    rclcpp::Subscription<std_msgs::msg::Int16>::SharedPtr subscription_hp;
    rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr subscription_state;
    size_t count_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<PlanningModule>());
  rclcpp::shutdown();
  return 0;
}